from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessIO
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
import os

def generate_launch_description():
    # UR机械臂控制节点
    ur_control_launch = ExecuteProcess(
        cmd=['agiros', 'launch', 'ur_robotiq_moveit_config', 'ur_robotiq_control.launch.py',
             'use_fake_hardware:=false', 'ur_type:=ur5', 'robot_ip:=192.168.0.101', 'launch_rviz:=false'],
        output='screen'
    )

    # 定义其他进程但不立即加入LaunchDescription
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('ur_robotiq_moveit_config'),
                        'launch/ur_robotiq_moveit.launch.py')
        ]),
        launch_arguments={
            'use_fake_hardware': 'false',
            'ur_type': 'ur5',
            'launch_rviz': 'true'
        }.items()
    )

    camera_launch = ExecuteProcess(
        cmd=['agiros', 'launch', 'astra_camera', 'astra.launch.xml'],
        output='screen',
        name='camera_launch_process'
    )

    aruco_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('ur5_moveit_demo'),
                        'launch/single.launch.py')
        ]),
        launch_arguments={
            'marker_id': '100',
            'marker_size': '0.1',
            'camera_frame': 'camera_link',
            'camera_info_topic': '/camera/color/camera_info',
            'image_topic': '/camera/color/image_raw',
            'marker_frame': 'aruco_marker_frame',
            'reference_frame': 'camera_link',
            'corner_refinement': 'LINES'
        }.items()
    )

    # easy_handeye2标定节点
    handeye_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory('easy_handeye2'),
                        'launch/calibrate.launch.py')
        ]),
        launch_arguments={
            'name': 'eye2hand_camera',
            'calibration_type':'eye_on_base',
            'tracking_base_frame':'camera_link',
            'tracking_marker_frame':'aruco_marker_frame',
            'robot_base_frame':'base_link',
            'robot_effector_frame':'tool0'
        }.items()
    )

    rqt_process = ExecuteProcess(
        cmd=['rqt', '--perspective-file', 
             os.path.join(get_package_share_directory('ur5_moveit_demo'),
                         'config/aruco.perspective')],
        output='screen'
    )

    # 定义事件处理函数
    def check_ur_output(event):
        # 解码输出并检查目标字符串
        try:
            text = event.text.decode('utf-8', errors='replace')
        except UnicodeDecodeError:
            return []
        if 'Robot connected to reverse interface. Ready to receive control commands.' in text:
            # 返回需要启动的后续进程
            return [moveit_launch, camera_launch, aruco_launch, handeye_launch, rqt_process]
        return []

    # 注册事件处理器，监听所有进程的stdout输出
    event_handler = RegisterEventHandler(
        event_handler=OnProcessIO(
            target_action=ur_control_launch,
            on_stdout=check_ur_output,
            #handle_once=True  # 确保只触发一次
        ),
    )

    # LaunchDescription中仅包含ur_control_launch和事件处理器
    return LaunchDescription([
        ur_control_launch,
        event_handler,
    ])


